#include "CCircle.h"


 


boolean	CCircle::Intersects( const CVector2d& translation, fixed radiusSquared,
							 const CLineSegment2d& l, 
							 CVector2d& closestPoint, 
							 CSpace::Relativity* relativity )
{
	fixed u;
	CVector2d toC;

	l.ShortestVectorToPoint( translation, toC, u ); 

	boolean intersecting = ( toC * toC <= radiusSquared )? TRUE : FALSE;

	closestPoint = translation - toC;

	if( relativity )
	{
		if( intersecting )
			*relativity = CSpace::Intersecting;
		else
			*relativity = l.DetermineSpace( translation );
	}

	return intersecting;
}


boolean	CCircle::Intersects( const CVector2d& translation, fixed radius, fixed radiusSquared,
							 const CVector2d& nextTranslation,
							 const CLineSegment2d& l,
							 fixed& u )
{
	CVector2d dv0;
	boolean intersecting = FALSE;	
		
	u = 0;

	l.ShortestVectorToPoint( translation, dv0, u );

	fixed d0 = dv0 * dv0;

	if( d0 <= radiusSquared )
		intersecting = TRUE;
	else
	{
		CVector2d dv1;
		
		l.ShortestVectorToPoint( nextTranslation, dv1, u );

		fixed d1 = dv1 * dv1;		

		if( d1 <= radiusSquared || dv0 * dv1 < 0 )
		{
			u = CMathFixed::Div( d0 - radiusSquared, d0 + d1 );
			intersecting = TRUE;
		}
	}

	return intersecting;	
}


//boolean CCircle::Intersects( const CVector2d& translation, fixed radius,
//							 const CVector2d& direction, fixed speed,
//							 const CLineSegment2d& l,
//							 fixed& u )
//{
//	return Intersects( translation, radius, translation + u * direction, l, u );
//}


boolean CCircle::DetectAndResolveCollision( const CVector2d& translation, fixed radius, fixed radiusSquared,
											const CLineSegment2d& l,
											CVector2d& newTranslation, fixed& u,
											CollisionResolution resolution,
											const CVector2d* lNormalPreComputed ) 
{
	boolean intersection;

	if( CCircle::Intersects( translation, radius, radiusSquared, newTranslation, l, u ) )
	{
		CVector2d t1t0 = newTranslation - translation;

		switch( resolution )
		{
		case Stop:
			newTranslation = translation + u * t1t0;
			break;
		case Slide:
			if( u < CMathFixed::One )
			{
				CVector2d tnT = newTranslation;
				newTranslation = translation + u * t1t0;

				if( lNormalPreComputed )
					newTranslation += ( ( tnT - newTranslation ) * *lNormalPreComputed ) * *lNormalPreComputed;
				else
				{
					CVector2d n = l.m_v1 - l.m_v0;
					newTranslation += ( ( tnT - newTranslation ) * n.Normalize() ) * n;
				}
			}
			break;
		default:
			ASSERT( FALSE ); // Not yet implemented
		}

		intersection = TRUE;
	}
	else
		intersection = FALSE;

	return intersection;
}


boolean CCircle::DetectAndResolveCollisionConvextArea( const CVector2d& translation, fixed radius,
													   const CVector2d* vtx, int32 nOfVertices,
													   CVector2d& newTranslation,
													   CollisionResolution resolution,
													   const CVector2d* lNormalPreComputed )
{
	fixed u;
	CVector2d* v0; 
	CVector2d* v1;
	int32 cnt = 0;
	fixed radiusSquared = CMathFixed::Mul( radius, radius );

	for( int32 i = 0; i < nOfVertices; i++ )
	{
		CLineSegment2d::ExtractSuccessiveVertices( (CVector2d*)vtx, nOfVertices, i, &v0, &v1 );
		CLineSegment2d l( *v0, *v1 );

		if( CCircle::DetectAndResolveCollision( translation, radius, radiusSquared, l, newTranslation, u,
												resolution, lNormalPreComputed ) )
		{
			cnt++;

			if( cnt > 1 )
				break;
		}
	}
	
	return ( cnt )? TRUE : FALSE;
}


boolean CCircle::ContainInsideConvexArea( const CVector2d& translation, fixed radius,
										  const CVector2d* vtx, int32 nOfVertices,
										  CVector2d& newTranslation )
{
	ASSERT( vtx );
	
	boolean success;
	CVector2d* v0; 
	CVector2d* v1;
	CVector2d vn;
	CVector2d toCpPoss;
	CVector2d toCp( 0, 0 );
	fixed toCpLen = MAX_INT32;
	CLineSegment2d l;
	CSpace::Relativity currRel = CSpace::Inside;
	CSpace::Relativity relPoss;
	fixed radiusSquared = CMathFixed::Mul( radius, radius );
	//fixed diameter = radius << 1;
	int32 skipLn = -1;
	
	newTranslation = translation;

int cnt = 0;//+hack, for now
	do
	{
		success = TRUE;

		for( int32 i = 0; i < nOfVertices; i++ )
		{
			if( i != skipLn )
			{
				CLineSegment2d::ExtractSuccessiveVertices( (CVector2d*)vtx, nOfVertices, i, &v0, &v1 );

				CLineSegment2d lPoss( *v0, *v1 );

				CCircle::Intersects( newTranslation, radiusSquared, lPoss, toCpPoss, &relPoss ); 

				if( relPoss != CSpace::Inside )
				{			
					toCpPoss = toCpPoss - newTranslation;

					fixed toCpPossLen = toCpPoss.Length();
					if( toCpPossLen && toCpPossLen < toCpLen )
					{
						currRel = relPoss;
						toCpLen = toCpPossLen;
						toCp = toCpPoss;
						l = lPoss;
						skipLn = i;
					}

					success = FALSE;
				}
			}
		}

cnt++;//+hack, for now

		if( !success )
		{
			fixed u;
			
			if( l.DetermineSpace( newTranslation ) != CSpace::Inside )
				u = toCpLen + radius;
			else
				u = toCpLen - radius;

			newTranslation += u * ( toCp / toCpLen );
		}
	}
	while( !success && cnt < 20 );
	
	return success;	
}
